#include "../includes.h"


#define	TRAIM_ACCURACY_LIMIT 200


char SKIP_RECEIVER_ALARM_CONTROL;
char SKIP_CONTROL;

char SKIP_CALC;

char freq_stable_enough;

int glov_off;
int glov_last;

RcvAlarm_ 	RcvAlarm;

void CheckReceiverAlarm(int);
void CheckReceiverGood(int);
void monitor_traim(void);



void Init_Ctrlmon()
{
	SKIP_CONTROL = OFF;

	RcvAlarm.prev_offset = 0;
	RcvAlarm.alarm_flag = OFF;
	RcvAlarm.ALARM_CONTROL = OFF;
	RcvAlarm.offset_index = 0;
	RcvAlarm.skip_counter = 0;
	RcvAlarm.start_flag = OFF;

	RcvAlarm.alarm_counter = 0;

	SKIP_RECEIVER_ALARM_CONTROL = ON;
	freq_stable_enough = NO;
	SKIP_CALC = NO;
}


int online_alarm_range;
int receiver_offset;

void CheckReceiverGood(int Offset)
{
	int diff;
	int alarm_range;

	if( RcvAlarm.ALARM_CONTROL == OFF ) { return; }

	if( SYSTEM.EB.STATUS != EB_STATUS_RUN) { return; }

	//if( KInterval < Kinterval_STEP_2 ) { return; }
	receiver_offset = Offset;
	
	diff = (RcvAlarm.last_offset - Offset);

	if(KInterval == Kinterval_STEP_4){
		alarm_range = RECEIVER_ALARM_RECOVERY_RANGE + (GC.RunningTime-RcvAlarm.alarm_start_time)*0.083*3;
	}
	else if(KInterval == Kinterval_STEP_3){
		alarm_range = RECEIVER_ALARM_RECOVERY_RANGE + (GC.RunningTime-RcvAlarm.alarm_start_time)*0.5*2;
	}
	else {
		alarm_range = RECEIVER_ALARM_RECOVERY_RANGE + (GC.RunningTime-RcvAlarm.alarm_start_time)*2;
	}


	if(abs(RcvAlarm.prev_offset - Offset) > 400 ){
		RcvAlarm.good_start_time = GC.RunningTime;
	}



	if( diff > -alarm_range && diff < alarm_range) {
		RcvAlarm.alarm_flag = OFF;
	}



	if( GC.RunningTime - RcvAlarm.good_start_time > 60*60 ){
		RcvAlarm.alarm_flag = OFF;
		RcvAlarm.last_offset = Offset;
	}


	if( GC.RunningTime - RcvAlarm.good_start_last_time > 120*60 ){
		RcvAlarm.alarm_flag = OFF;

		InitializeKalmanControl();
		ChangeControlInterval(Kinterval_STEP_2);
		CONTROL.ControlDAC = CONTROL.RealDAC = CONTROL.PrevDAC = (double)CONFIG.DAC;
		MeanDAC = CONTROL.NominalDAC = CONTROL.RealDAC;

		RcvAlarm.start_flag = OFF;
	}


	if(RcvAlarm.alarm_flag == OFF){

		RcvAlarm.ALARM_CONTROL = OFF;
		//CONTROL.AvgTimeOffset = HoldoverCONTROL.AvgTimeOffset*(1E-9);

		//LogIt(LOG_Receiver_Good,SYSTEM.TRACK.BINSAT);
		//LogIt(LOG_Receiver_Good,((RcvAlarm.prev_offset<<16)&0xFFFF0000) | (Offset&0xFFFF) );
		glov_off = Offset;
		online_alarm_range = alarm_range;
		
		//log_1pps_online();
		sprintf(MESG,"1PPS online, off:%dns, ref:%dns, dac:%d, tdac:%d, temp:%d",
			glov_off, online_alarm_range, (int)CONTROL.RealDAC, (int)OCXO_ID.TDAC, (int)TEMPERATURE.CURRENT
		);
		LogItString(LOG_CATA1_NORM, LOG_CATA2_OK, MESG);

		RECOVERY.flag = ON;
		RECOVERY.IntervalCounter = 0;
		RECOVERY.wait_time = 100;
		RECOVERY.average_time = 100;

		MessageOut("\r\n--Online---\r\n");
	}


	RcvAlarm.prev_offset = Offset;


	sprintf((char*)MESG,"CheckOn> KI:%d Off:%d LO:%d diff:%d ar:%d time:%d\r\n",
		KInterval,Offset ,RcvAlarm.last_offset ,diff ,alarm_range ,GC.RunningTime - RcvAlarm.good_start_time);
	MessageOut(MESG);
}

void CheckReceiverAlarm(int Offset)
{
	int i,offset,diff,sum;
	int alarm_range;

	if( RcvAlarm.ALARM_CONTROL == ON ) { 		return;	}

	if( SYSTEM.EB.STATUS != EB_STATUS_RUN) { 	return;	}

	if (RcvAlarm.skip_counter > 0 ) {
		RcvAlarm.skip_counter--;
		RcvAlarm.offset_index = 0;
		return;
	}

	if((RcvAlarm.start_flag == OFF) || (KInterval < Kinterval_STEP_2)) {
		RcvAlarm.offset_index = 0;
		return;
	}

	RcvAlarm.good_start_time = GC.RunningTime;
	RcvAlarm.good_start_last_time = GC.RunningTime;


	RcvAlarm.offset_table[RcvAlarm.offset_index++] = Offset;

	if(RcvAlarm.offset_index >= 10){
		
		for( i = 0, sum = 0; i < RcvAlarm.offset_index; i++ ){
			sum += RcvAlarm.offset_table[i];
		}
		offset = sum/RcvAlarm.offset_index;

		RcvAlarm.offset_index = 0;

	
		if(RECOVERY.flag == ON){

			diff = offset - RcvAlarm.last_offset; 
			glov_last = RcvAlarm.last_offset;
		}
		else {

			diff = offset - CONTROL.AvgTimeOffset*1E9;
			glov_last = CONTROL.AvgTimeOffset*BILLION;
		}


		if(KInterval == Kinterval_STEP_4){
			alarm_range = RECEIVER_ALARM_ESCAPE_RANGE + (GC.RunningTime-RcvAlarm.alarm_start_time)*0.083*3;
		}
		else if(KInterval == Kinterval_STEP_3){
			alarm_range = RECEIVER_ALARM_ESCAPE_RANGE + (GC.RunningTime-RcvAlarm.alarm_start_time)*0.5*2;
		}
		else{
			alarm_range = RECEIVER_ALARM_ESCAPE_RANGE + (GC.RunningTime-RcvAlarm.alarm_start_time)*2;
		}


		if( diff > alarm_range || diff < -alarm_range) {
			RcvAlarm.alarm_flag = ON;
			RcvAlarm.ALARM_CONTROL = ON;

			SKIP_CONTROL = ON;
			SKIP_RECEIVER_ALARM_CONTROL = ON;
			
			freq_stable_cnt=0;
			freq_stable_enough = NO;

			//LogIt(LOG_Receiver_Alarm,SYSTEM.TRACK.BINSAT);
			//LogIt(LOG_Receiver_Alarm,((((int)(CONTROL.AvgTimeOffset*1E9))<<16)&0xFFFF0000) | (offset&0xFFFF) );
			glov_off = offset;
			
			//log_1pps_offline();
			sprintf(MESG,"1PPS offline, off:%dns -> %dns, dac:%d, tdac:%d, temp:%d",
				glov_last, glov_off, (int)CONTROL.RealDAC, (int)OCXO_ID.TDAC, (int)TEMPERATURE.CURRENT
			);
			LogItString(LOG_CATA1_NORM, LOG_CATA2_WARN, MESG);
			
			skipping_change_eb = 1;
			if( CONFIG.RcvType==1 || 1){
				LogIt3(LOG_CATA1_NORM, LOG_CATA2_WARN, LOG_SYNC15_RESET, RESET_MODEL_BOTH, RESET_TYPE_INITSTEP, 13);
				Init_EB(INIT_EB_SILENT, INIT_EB_SECOND, YES);
				EB_Task_start();
			}
			MessageOut("\r\n----Offline------------\r\n");
			
			MessageOut("\r\nCheckVal> ");
			if( SYSTEM.IO.CTRL_PORT.MODE == UDEBUG ){
				for( i = 0; i < 10; i++ ){
					sprintf((char*)MESG,"%d:%d ", i, RcvAlarm.offset_table[i]);
					TransmitMessage((BYTE*)MESG,PORT_CONTROL);
				}
			}
			
			sprintf((char*)MESG,"\r\nCheckOff> KI:%d off:%d Coff:%d diff:%d ar:%d LO:%d\r\n",
				KInterval ,offset ,(int)(CONTROL.AvgTimeOffset*1E9) ,diff ,alarm_range ,RcvAlarm.last_offset);
			MessageOut(MESG);
		}
	}
}

int traim_status_prev = 0;

void monitor_traim(void)
{
	int traim_status_now;
	
	if(SYSTEM.EB.TRAIM.STATUS == 0 || SYSTEM.EB.TRAIM.STATUS == 1){
		if(SYSTEM.EB.TRAIM.SOL_STATUS == 1) {
			traim_status_now = 1;
			
			if(traim_status_prev != traim_status_now){
				TLogIt(TLOG_TRAIM_ALM, 
					(SYSTEM.EB.TRAIM.ACCURACY<<16&0xFFFF0000) |
					(SYSTEM.EB.TRAIM.SAWTOOTH<<8&0x0000FF00) |
					(SYSTEM.TRACK.TRACKED&0x000000FF)
					);
			}
		}
		else if(SYSTEM.EB.TRAIM.SOL_STATUS == 2) {
			traim_status_now = 2;
			
			if(traim_status_prev != traim_status_now){
				TLogIt(TLOG_TRAIM_UNKNOWN, 
					(SYSTEM.EB.TRAIM.ACCURACY<<16&0xFFFF0000) |
					(SYSTEM.EB.TRAIM.SAWTOOTH<<8&0x0000FF00) |
					(SYSTEM.TRACK.TRACKED&0x000000FF)
					);
			}
		}
		else{
			traim_status_now = 0;
			
			if(traim_status_prev != traim_status_now){
				TLogIt(TLOG_TRAIM_OK, 
					(SYSTEM.EB.TRAIM.ACCURACY<<16&0xFFFF0000) |
					(SYSTEM.EB.TRAIM.SAWTOOTH<<8&0x0000FF00) |
					(SYSTEM.TRACK.TRACKED&0x000000FF)
					);
			}
		}
		
		traim_status_prev = traim_status_now;
	}
	
	if(SYSTEM.EB.TRAIM.STATUS == 2){
		if(SYSTEM.EB.STATUS == EB_STATUS_RUN && SYSTEM.OPERATION.OPMODE == OPMODE_OPERATION &&
			turn == 0/*lock_control*/){
			traim_status_now = 3;
			if(traim_status_prev != traim_status_now){
				TLogIt(TLOG_TRAIM_ERROR, 
						(SYSTEM.EB.TRAIM.ACCURACY<<16&0xFFFF0000) |
						(SYSTEM.EB.TRAIM.SAWTOOTH<<8&0x0000FF00) |
						(SYSTEM.TRACK.TRACKED&0x000000FF)
						);
			}
			traim_status_prev = traim_status_now;
		}
	}
}


